/*
 * @file engine_controller.h
 *
 * @brief control the platform.
 * @note based on the DPR2 code.
 * @author Michiel Hegemans
 * @author Peter Evers
 *
 * @date 5 Dec 2012
 */

#ifndef ENGINECONTROLLER_H_
#define ENGINECONTROLLER_H_

#include <ros/ros.h>
#include <threemxl/C3mxlROS.h>
#include <threemxl/dxlassert.h>
#include <XMLConfiguration.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float32.h>

#include "topic/topic.h"

class EngineController {
protected:
	ros::NodeHandle nh_; ///< ROS node handle
	ros::Subscriber vel_sub_; ///< Subscriber for velocity messages
	ros::Subscriber posX_sub_; ///< Subscriber for position messages
	ros::Subscriber posZ_sub_; ///< Subscriber for position messages
	ros::Publisher vel_pub_; ///< Publisher for velocity messages

	C3mxlROS *left_motor_; ///< Left motor handle
	C3mxlROS *right_motor_; ///< Right motor handle
	double wheel_diameter_; ///< Wheel diameter in meters
	double wheel_base_; ///< Wheel base in meters

	double Kp_;
	double Ki_;
	double Kd_;
	double Ki_limit_;

	int motor_mode_;
protected:
	/// Called when a new velocity command is published
	/** Sends the new velocity to the base motors
	  * @param msg Pointer to geometry_msgs/Twist message, containing the linear and angular velocities, in [m/s] and [rad/s] respectively.
	  * @note Since the base is nonholonomic, only linear velocities in the x direction and angular velocities around the z direction are supported.
	  */
	void velocityCallback(const geometry_msgs::Twist::ConstPtr &msg);

	/// Called when a new position command is published
	/** Sends the new position to the base motors
	  * @param msg
	  */
	void positionZCallback(const std_msgs::Float32::ConstPtr &msg);

	/// Called when a new position command is published
	/** Sends the new position to the base motors
	  * @param msg
	  */
	void positionXCallback(const std_msgs::Float32::ConstPtr &msg);

	/// Called when a new velocity command is published
	/** Sends the new velocity to the base motors
	  * @param msg Pointer to geometry_msgs/Twist message, containing the linear and angular velocities, in [m/s] and [rad/s] respectively.
	  * @note Since the base is nonholonomic, only linear velocities in the x direction and angular velocities around the z direction are supported.
	  */
	void publishMeasuredVelocity();

	void setPosition(double left, double right, double speed);

public:
	EngineController();

	~EngineController();

	void init();
	void spin();
};


#endif /* ENGINECONTROLLER_H_ */
